# -*- coding: utf-8 -*-
"""
Created on Sun Sep  8 01:25:11 2024

@author: LENOVO
"""

# -*- coding: utf-8 -*-
"""
Created on Sat Sep  7 12:28:57 2024

@author: LENOVO
"""
import matplotlib.pyplot as plt
import numpy as np
from sympy import *
from scipy.optimize import root, fsolve
import pandas as pd
import math

H=1.7
a=16*H  #m
b=H/(2*np.pi) #m
vh=1 #m/s
L0=(341-27.5*2)/100
Lb=(220-27.5*2)/100

r1=lambda theta:((a-b*(theta)))
# r2=lambda theta:((a-b*(theta+np.pi)))

f1=lambda theta:((a-b*(theta)))-4.5
theta1=root(f1,0).x[0] #进入调头空间角度
#进入掉头空间时间
t1=(b*theta1-a)*np.sqrt((a-b*theta1)**2+b**2)/(2*b)-0.5*b*np.log(np.sqrt((a-b*theta1)**2+b**2)+a-b*theta1)+a/(2*b)*np.sqrt(a**2+b**2)+0.5*b*np.log(a+np.sqrt(a**2+b**2))
#进入掉头空间时 龙头位置

#某点斜率函数
def Slope(ta):
    k=(-a*np.cos(ta)+b*np.sin(ta)+b*ta*np.cos(ta))/(-a*np.sin(ta)-b*np.cos(ta)+b*ta*np.sin(ta))
    return k
# theta=np.linspace(0,32*np.pi,30*180)


# ax=plt.subplot(111, polar=True)
# ax.set_theta_direction(-1)
# plt.plot(theta,r(theta),lw=1,c='r', label='原始数据点')
# plt.legend()
# plt.title('非线性最小二乘拟合')
# plt.grid()  
# plt.show()




thetas=lambda t:(a-np.sqrt(a**2-2*b*vh*t))/b #猜测角度值

def theta(t):
    f=lambda x:(b*x-a)*np.sqrt((a-b*x)**2+b**2)/(2*b)-0.5*b*np.log(np.sqrt((a-b*x)**2+b**2)+a-b*x)+a/(2*b)*np.sqrt(a**2+b**2)+0.5*b*np.log(a+np.sqrt(a**2+b**2))-t
    theta=root(f,thetas(t))
    theta=theta.x[0]
    return theta


def position(i):
    
    r=r1
    Data=[]
    THETA=[]
    X=[]
    Y=[]
    # y=np.ones((301,1))
    
    THETA.append(theta(i))
    X.append(r(THETA[0])*np.cos((THETA[0])))
    Y.append(-r(THETA[0])*np.sin((THETA[0])))

    Data.append(theta(i))
    f=lambda thetai:(r(THETA[0]))**2+(r(thetai))**2-L0**2-2*(r(THETA[0]))*(r(thetai))*np.cos(THETA[0]-thetai)
    thetai=root(f,THETA[0]-0.5)
    thetai=thetai.x[0]
    
    # print("theta1:",thetai)
    THETA.append(thetai)
    X.append(r(THETA[1])*np.cos((THETA[1])))
    Y.append(-r(THETA[1])*np.sin((THETA[1])))
    Data.append(thetai)
    
    for j in range(0,222,1):
        
        f=lambda thetai:(r(THETA[1+j]))**2+(r(thetai))**2-Lb**2-2*(r(THETA[1+j]))*(r(thetai))*np.cos(THETA[1+j]-thetai)
        thetai=root(f,THETA[1+j]-0.5)
        thetai=thetai.x[0]
        THETA.append(thetai)
        X.append(r(THETA[j+2])*np.cos((THETA[j+2])))
        Y.append(-r(THETA[j+2])*np.sin((THETA[j+2])))

    A=np.column_stack((X, Y, THETA))

    return A

#第ts开始进入调头曲线
def Turn(t):
    theta0=theta(t)
    P0=position(t)
    x0=P0[0,0]
    y0=P0[0,1]
    k0=Slope(theta0)
    k1=-1/(y0/x0)
    tan=(k1-k0)/(1+k0*k1)
    fai=np.arctan(abs(tan))
    R=r1(theta0)/(3*np.cos(fai))
    fo1=lambda x:[(y0-x[1])/(x0-x[0])+k0,(x0-x[0])**2+(y0-x[1])**2-(2*R)**2]
    o1=fsolve(fo1,[x0-0.1,y0-0.1])
    fo2=lambda x:[(-y0-x[1])/(-x0-x[0])+k0,(-x0-x[0])**2+(-y0-x[1])**2-(R)**2]
    o2=fsolve(fo2,[-x0-0.1,-y0-0.1])
    o=np.vstack((o1,o2)) #两个圆心原点坐标 第一个为2R 第二个为R
    
    
    #大圆圆心坐标系下ABC三点坐标
    xA=-2*R*np.cos(fai)
    yA=2*R*np.sin(fai)
    xB=2*R*cos(fai)
    yB=2*R*np.sin(fai)
    xC=4*R*np.cos(fai)
    yC=2*R*np.sin(fai)
    
    #坐标变换矩阵 
    B=np.array([[-x0/r1(theta0),y0/r1(theta0)],[-y0/r1(theta0),-x0/r1(theta0)]])
    #螺旋线到圆弧坐标
    def LTY(Y):
        L=B@Y+o1.T
        return L
    # return(LTY(np.array([xA,yA]))) #测试点
    
    #时间节点
    t0=t
    t1=2*R*(np.pi-2*fai)/vh+t0
    t2=R*(np.pi-2*fai)/vh+t1
    deltat=t2-t0
    #龙头位置：龙头在螺旋线坐标系下坐标 
    def Loonghead (t):
        #未进入圆弧前 
        if (t>=0 and t<t0):
            P=position(t)
            x=P[0,0]
            y=P[0,1]
            GPS=np.array([x,y])
        #第一段圆弧
        elif (t>=t0 and t<t1):
            x=2*R*np.cos(np.pi-fai-vh*(t-t0)/(2*R))
            y=2*R*np.sin(np.pi-fai-vh*(t-t0)/(2*R))
            ZB=np.array([x,y])
            GPS=LTY(ZB.T)
        #第二段圆弧    
        elif (t>=t1 and t<t2):
            x=R*np.cos(np.pi-fai-vh*(t-t1)/R)+3*R*np.cos(fai)
            y=-R*np.sin(np.pi-fai-vh*(t-t1)/R)+3*R*np.sin(fai)
            ZB=np.array([x,y])
            GPS=LTY(ZB.T)
        #出圆弧后 由中心对称可知
        else:
            P=position(t0+t2-t)
            x=-P[0,0]
            y=-P[0,1]
            GPS=np.array([x,y])
        return GPS
    LTQ=[]
    for i in range(0,int(t)+1,1):
        LTQ.append(Loonghead(i))
    return LTQ #测试点

LTQ=Turn(1300)
LTQ=pd.DataFrame(LTQ)
LTQ.to_excel('1300.xlsx',index=False,header=False)
            
            
            
    
        
        
        
    # return o
    
    
    
    